/**
 ******************************************************************************
 * @FilePath: \for_infantry\Module\Gimbal.c
 * @Brief: 
 * @Date: 2021-01-12 23:31:03
 * @Author: Rio
 ******************************************************************************
 */
#include "Gimbal.h"
#include "Chassis.h"
#include "bsp_usart.h"
#include "bsp_can.h"
#include "remoteCtrl.h"
#include "Motor_control.h"
#include "pid.h"
#include "can.h"
#include "nuc_interface.h"
#include "Mode_SW.h"
#include <string.h>
#include <math.h>
#include "Chassis.h"
#include "user_lib.h"
#include "kalman_filter.h"

/*********************************** Private defines *****************************************/

/***********************电机各控制环PID具体参数*****************************/
#define PIT_ENCODE_MID_R          8.3429f
#define PIT_ENCODE_UP_LIM         40.0f
#define PIT_ENCODE_DOWN_LIM       -17.0634f
#define PIT_GYRO_UP_LIM           30.45f
#define PIT_GYRO_DOWN_LIM					-19.3f
#define PIT_GYRO_MID              4.15f

#define VISION_X_CENTRE         762.0f
#define VISION_Y_CENTRE         206.0f
#define GIMBAL_DEBUG             1
/********************************** Private variables ****************************************/
motorData_t s_gimbal_yaw_motor = {0};
PID_AbsoluteType s_gimbal_yaw_motor_spd_pid = {0};
PID_AbsoluteType s_gimbal_yaw_motor_pos_pid = {0};
PID_AbsoluteType s_gimbal_yaw_encodmotor_spd_pid = {0};
PID_AbsoluteType s_gimbal_yaw_encodmotor_pos_pid = {0};
PID_AbsoluteType s_gimbal_yaw_vision_spd_pid = {0};
PID_AbsoluteType s_gimbal_yaw_vision_pos_pid = {0};

motorData_t s_gimbal_pit_motor = {0};
PID_AbsoluteType s_gimbal_pit_motor_spd_pid = {0};
PID_AbsoluteType s_gimbal_pit_motor_pos_pid = {0};
PID_AbsoluteType s_gimbal_pit_encodmotor_spd_pid = {0};
PID_AbsoluteType s_gimbal_pit_encodmotor_pos_pid = {0};
PID_AbsoluteType s_gimbal_pit_vision_pos_pid = {0};
PID_AbsoluteType s_gimbal_pit_vision_spd_pid = {0};

visInf_t s_visionInform = {0};
/********************************** Extern variables ****************************************/
extern remoteCtrl_t s_rcData;
extern filterLow_t s_pitFilter, s_pitOffset, s_yawFilter;
extern kalman1_state s_yawKalmanFil,s_dtaxKalmanFil;
extern PC_DATA pcData;
extern float dtaX, lastX, lastY;
extern float p,i,d,S1;
extern float P,I,D,S2;
/********************************** Static function ****************************************/

/********************************** Interface function ****************************************/
void Gimbal_Init(void)
{
	memset(&s_gimbal_yaw_motor, 0, sizeof(motorData_t));//对电机信息结构体进行清零初始化
	memset(&s_gimbal_pit_motor, 0, sizeof(motorData_t));//对电机信息结构体进行清零初始化
	s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_MID_R;
	// s_gimbal_pit_motor.PositionTarget = PIT_GYRO_MID;
	s_gimbal_yaw_motor.PositionTarget = s_gimbal_yaw_motor.gyro_angle;

	PID_AbsParam_Init(&s_gimbal_yaw_motor_pos_pid, 30,  0, 0, 0, 1000);
	PID_AbsParam_Init(&s_gimbal_yaw_motor_spd_pid, 170, 0, 1, 20000, 28000);

	PID_AbsParam_Init(&s_gimbal_yaw_encodmotor_pos_pid, 110, 0, 0, 0, 500);
	PID_AbsParam_Init(&s_gimbal_yaw_encodmotor_spd_pid, 80, 0, 0, 0, 28000);

	PID_AbsParam_Init(&s_gimbal_pit_motor_pos_pid, 20, 0, 0, 100, 800);
	PID_AbsParam_Init(&s_gimbal_pit_motor_spd_pid, 100, 0, 0, 20000, 28000);// 0.9

	PID_AbsParam_Init(&s_gimbal_pit_encodmotor_pos_pid, 10, 0, 0, 0, 100);
	PID_AbsParam_Init(&s_gimbal_pit_encodmotor_spd_pid, 120, 0, 0, 2000, 28000);

	PID_AbsParam_Init(&s_gimbal_yaw_vision_pos_pid, 0.8f, 0.0, 0, 0, 800);
	PID_AbsParam_Init(&s_gimbal_yaw_vision_spd_pid, 25.0f, 0.4, 1.0f, 0, 28000);

	PID_AbsParam_Init(&s_gimbal_pit_vision_pos_pid, 0.7f, 0, 0, 0, 100);
	PID_AbsParam_Init(&s_gimbal_pit_vision_spd_pid, 150.0f, 0.0f, 0, 2000, 28000);

	s_visionInform.yawCenter = VISION_X_CENTRE;
	s_visionInform.pitchCenter = VISION_Y_CENTRE;
	
	s_pitFilter.alpha = 1.0f;
	s_pitOffset.alpha = 0.8f;
	s_yawFilter.alpha = 0.8f;
	kalman1_init(&s_yawKalmanFil,0,500,1,50); 
	kalman1_init(&s_dtaxKalmanFil,0, 100, 5, 50); 	
	start_motor();
}

void Gimbal_Ctrl(void)
{
//	PID_AbsParam_Set(&s_gimbal_yaw_vision_pos_pid, p, i, d, 0, S1);
//	PID_AbsParam_Set(&s_gimbal_yaw_vision_spd_pid, P, I, D, 20000, 28000);
	static int16_t gimbalYawCtrl , gimbalPitCtrl;

	switch(roboSet.e_aimingMode)
	{
		case G_MANUAL:
		{
			s_gimbal_yaw_motor.PositionTarget -= ((s_rcData.rc.ch[2] - 1024)*0.048f*0.3*0.1 \
																						+s_rcData.mouse.x*0.1*0.5 \
																						-roboSet.turnBack * 180.0f);
			gimbalYawCtrl = -motor_cascade_loop_pid_control(&s_gimbal_yaw_motor_pos_pid, \
																											&s_gimbal_yaw_motor_spd_pid, \
																											s_gimbal_yaw_motor.gyro_angle, \
																											s_gimbal_yaw_motor.PositionTarget, \
																											s_gimbal_yaw_motor.gyro_speed*2);
			s_gimbal_yaw_motor.yawEncodTarget = s_gimbal_yaw_motor.position_angle;
		//陀螺仪控制
			// s_gimbal_pit_motor.PositionTarget += ((s_rcData.rc.ch[3] - 1024)*0.0012f*0.45f-s_rcData.mouse.y*0.04);
			// if(s_gimbal_pit_motor.PositionTarget < PIT_ENCODE_DOWN_LIM) s_gimbal_pit_motor.PositionTarget = PIT_ENCODE_DOWN_LIM;
			// else if(s_gimbal_pit_motor.PositionTarget > PIT_ENCODE_UP_LIM) s_gimbal_pit_motor.PositionTarget = PIT_ENCODE_UP_LIM;
			// gimbalPitCtrl = -motor_cascade_loop_pid_control(&s_gimbal_pit_motor_pos_pid, \
			// 																						&s_gimbal_pit_motor_spd_pid, \
			// 																						(s_gimbal_pit_motor.gyro_angle), \
			// 																						s_gimbal_pit_motor.PositionTarget, \
			// 																					-s_gimbal_pit_motor.gyro_speed);
			s_gimbal_pit_motor.pitEncodTarget += ((s_rcData.rc.ch[3] - 1024)*0.0012f*0.45f-s_rcData.mouse.y*0.03);
			if(s_gimbal_pit_motor.pitEncodTarget > PIT_ENCODE_UP_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_UP_LIM;
			else if(s_gimbal_pit_motor.pitEncodTarget < PIT_ENCODE_DOWN_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_DOWN_LIM;
			gimbalPitCtrl = motor_cascade_loop_pid_control(&s_gimbal_pit_encodmotor_pos_pid, \
																										&s_gimbal_pit_encodmotor_spd_pid, \
																										s_gimbal_pit_motor.position_angle, \
																										s_gimbal_pit_motor.pitEncodTarget, \
																										s_gimbal_pit_motor.gyro_speed);
//			 s_gimbal_pit_motor.pitEncodTarget = s_gimbal_pit_motor.position_angle;
		}	break;

		case G_ENCODE:
		{
			s_gimbal_yaw_motor.yawEncodTarget += ((s_rcData.rc.ch[2] - 1024)*0.048f*0.1*0.1 \
																						+(s_rcData.key.v & CHASSIS_RIGHT_KEY)*0.0015 \
																						-(s_rcData.key.v & CHASSIS_LEFT_KEY)*0.0015 \
																						+s_rcData.mouse.x*0.1*0.15*s_rcData.mouse.pressRight);

			gimbalYawCtrl = motor_cascade_loop_pid_control(&s_gimbal_yaw_encodmotor_pos_pid, \
																											&s_gimbal_yaw_encodmotor_spd_pid, \
																											s_gimbal_yaw_motor.position_angle, \
																											s_gimbal_yaw_motor.yawEncodTarget, \
																											-s_gimbal_yaw_motor.gyro_speed*2);
			s_gimbal_yaw_motor.PositionTarget = s_gimbal_yaw_motor.gyro_angle;
			s_gimbal_yaw_motor.visTarget = s_gimbal_yaw_motor.gyro_angle;

			s_gimbal_pit_motor.pitEncodTarget += ((s_rcData.rc.ch[3] - 1024)*0.0012f*0.05f \
																								+(s_rcData.key.v & CHASSIS_FRONT_KEY)*0.01
																								-(s_rcData.key.v & CHASSIS_BACK_KEY)*0.01
																								-s_rcData.mouse.y*0.01*s_rcData.mouse.pressRight);
			if(s_gimbal_pit_motor.pitEncodTarget > PIT_ENCODE_UP_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_UP_LIM;
			else if(s_gimbal_pit_motor.pitEncodTarget < PIT_ENCODE_DOWN_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_DOWN_LIM;
			gimbalPitCtrl = motor_cascade_loop_pid_control(&s_gimbal_pit_encodmotor_pos_pid, \
																										&s_gimbal_pit_encodmotor_spd_pid, \
																										s_gimbal_pit_motor.position_angle, \
																										s_gimbal_pit_motor.pitEncodTarget, \
																										s_gimbal_pit_motor.gyro_speed);
			roboSet.turnBack = 0;																							
			// s_gimbal_pit_motor.PositionTarget = s_gimbal_pit_motor.gyro_angle;
		}	break;

		case G_VISION:
		{
			if((s_visionInform.yawPos > 1.0f && fabs(dtaX)<300.0f))
			{
				//__LASER_ON();//关闭镭射
				gimbalYawCtrl = motor_cascade_loop_pid_control(&s_gimbal_yaw_vision_pos_pid, \
																												&s_gimbal_yaw_vision_spd_pid, \
																												s_visionInform.yawCenter, \
																												s_gimbal_yaw_motor.visTarget, \
																												-s_gimbal_yaw_motor.gyro_speed*25);
				s_gimbal_yaw_motor.PositionTarget = s_gimbal_yaw_motor.gyro_angle;
				s_gimbal_yaw_motor.yawEncodTarget = s_gimbal_yaw_motor.position_angle;

				// s_gimbal_pit_motor.pitEncodTarget -= ((s_rcData.rc.ch[3] - 1024)*0.0012f*0.45f-s_rcData.mouse.y*0.04);
				// if(s_gimbal_pit_motor.pitEncodTarget > PIT_ENCODE_DOWN_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_DOWN_LIM;
				// else if(s_gimbal_pit_motor.pitEncodTarget < PIT_ENCODE_UP_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_UP_LIM;
				// gimbalPitCtrl = motor_cascade_loop_pid_control(&s_gimbal_pit_encodmotor_pos_pid, \
				// 																							&s_gimbal_pit_encodmotor_spd_pid, \
				// 																							s_gimbal_pit_motor.position_angle, \
				// 																							s_gimbal_pit_motor.pitEncodTarget, \
				// 																							s_gimbal_pit_motor.gyro_speed*0.5);


				gimbalPitCtrl = -motor_cascade_loop_pid_control(&s_gimbal_pit_vision_pos_pid, \
																											&s_gimbal_pit_vision_spd_pid, \
																											s_visionInform.pitchCenter*0.5, \
																											s_gimbal_pit_motor.visPitTarget*0.5, \
																											-s_gimbal_pit_motor.gyro_speed*2);
																										
				s_gimbal_pit_motor.pitEncodTarget = s_gimbal_pit_motor.position_angle;
			}
			else
			{
				s_gimbal_yaw_motor.PositionTarget -= ((s_rcData.rc.ch[2] - 1024)*0.048f*0.3*0.3+s_rcData.mouse.x*0.3*0.5);
				gimbalYawCtrl = -motor_cascade_loop_pid_control(&s_gimbal_yaw_motor_pos_pid, \
																												&s_gimbal_yaw_motor_spd_pid, \
																												s_gimbal_yaw_motor.gyro_angle, \
																												s_gimbal_yaw_motor.PositionTarget, \
																												s_gimbal_yaw_motor.gyro_speed*2);
				s_gimbal_yaw_motor.yawEncodTarget = s_gimbal_yaw_motor.position_angle;
//				s_gimbal_yaw_motor.PositionTarget = s_gimbal_yaw_motor.gyro_angle;

				s_gimbal_pit_motor.pitEncodTarget += ((s_rcData.rc.ch[3] - 1024)*0.0012f*0.45f-s_rcData.mouse.y*0.04);
				if(s_gimbal_pit_motor.pitEncodTarget > PIT_ENCODE_UP_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_UP_LIM;
				else if(s_gimbal_pit_motor.pitEncodTarget < PIT_ENCODE_DOWN_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_DOWN_LIM;
				gimbalPitCtrl = motor_cascade_loop_pid_control(&s_gimbal_pit_encodmotor_pos_pid, \
																											&s_gimbal_pit_encodmotor_spd_pid, \
																											s_gimbal_pit_motor.position_angle, \
																											s_gimbal_pit_motor.pitEncodTarget, \
																											s_gimbal_pit_motor.gyro_speed);
			}
		} break;
		
		case G_HAlf_VISION:
		{
			if((s_visionInform.yawPos > 1.0f && fabs(dtaX)<300.0f))
			{
				// s_gimbal_pit_motor.pitEncodTarget -= ((s_rcData.rc.ch[3] - 1024)*0.0012f*0.45f-s_rcData.mouse.y*0.04);
				// if(s_gimbal_pit_motor.pitEncodTarget > PIT_ENCODE_DOWN_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_DOWN_LIM;
				// else if(s_gimbal_pit_motor.pitEncodTarget < PIT_ENCODE_UP_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_UP_LIM;
				// gimbalPitCtrl = motor_cascade_loop_pid_control(&s_gimbal_pit_encodmotor_pos_pid, \
				// 																							&s_gimbal_pit_encodmotor_spd_pid, \
				// 																							s_gimbal_pit_motor.position_angle, \
				// 																							s_gimbal_pit_motor.pitEncodTarget, \
				// 																							s_gimbal_pit_motor.gyro_speed*0.5);


				gimbalPitCtrl = -motor_cascade_loop_pid_control(&s_gimbal_pit_vision_pos_pid, \
																											&s_gimbal_pit_vision_spd_pid, \
																											s_visionInform.pitchCenter*0.5, \
																											s_gimbal_pit_motor.visPitTarget*0.5, \
																											-s_gimbal_pit_motor.gyro_speed*2);
																										
				s_gimbal_pit_motor.pitEncodTarget = s_gimbal_pit_motor.position_angle;
			}
			else
			{
				s_gimbal_yaw_motor.PositionTarget -= ((s_rcData.rc.ch[2] - 1024)*0.048f*0.3*0.3+s_rcData.mouse.x*0.3*0.5);
				gimbalYawCtrl = -motor_cascade_loop_pid_control(&s_gimbal_yaw_motor_pos_pid, \
																												&s_gimbal_yaw_motor_spd_pid, \
																												s_gimbal_yaw_motor.gyro_angle, \
																												s_gimbal_yaw_motor.PositionTarget, \
																												s_gimbal_yaw_motor.gyro_speed*2);
				s_gimbal_yaw_motor.yawEncodTarget = s_gimbal_yaw_motor.position_angle;
//				s_gimbal_yaw_motor.PositionTarget = s_gimbal_yaw_motor.gyro_angle;

				s_gimbal_pit_motor.pitEncodTarget += ((s_rcData.rc.ch[3] - 1024)*0.0012f*0.45f-s_rcData.mouse.y*0.04);
				if(s_gimbal_pit_motor.pitEncodTarget > PIT_ENCODE_UP_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_UP_LIM;
				else if(s_gimbal_pit_motor.pitEncodTarget < PIT_ENCODE_DOWN_LIM) s_gimbal_pit_motor.pitEncodTarget = PIT_ENCODE_DOWN_LIM;
				gimbalPitCtrl = motor_cascade_loop_pid_control(&s_gimbal_pit_encodmotor_pos_pid, \
																											&s_gimbal_pit_encodmotor_spd_pid, \
																											s_gimbal_pit_motor.position_angle, \
																											s_gimbal_pit_motor.pitEncodTarget, \
																											s_gimbal_pit_motor.gyro_speed);
			}		
		}

		case G_NULL:
		{
      s_gimbal_yaw_motor.PositionTarget = s_gimbal_yaw_motor.gyro_angle;
			s_gimbal_yaw_motor.yawEncodTarget = s_gimbal_yaw_motor.position_angle;
			gimbalYawCtrl = 0;
			gimbalPitCtrl = 0;
		}	break;
	}

	CANTx_SendCurrent(&hcan1, 0x1FF, gimbalYawCtrl, 0, 0, 0);
	Pitch_SendCurrent(0x1, 0, 0, 0, 0, gimbalPitCtrl/2000);//positive->up
//	if(s_gimbal_pit_motor.esc_back_temperature < 80)
//		CANTx_SendCurrent(&hcan2, 0x1FF, gimbalPitCtrl, 0, 0, 0);//positive->down
//	else
//		CANTx_SendCurrent(&hcan2, 0x1FF, 0, 0, 0, 0);
}